function T = FKine_six(q, DH_para)
Alpha = DH_para.Alpha;
A = DH_para.A;
D = DH_para.D;
Theta = DH_para.Theta;
T = eye(4);
for i = 1:length(q)
    T = T * TransformationMatrixMDH(Alpha(i), A(i), D(i), Theta(i), q(i));
end
